/**                   GNU GENERAL PUBLIC LICENSE
 *                       Version 3, 29 June 2007
 *
 * Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
 * Everyone is permitted to copy and distribute verbatim copies
 * of this license document, but changing it is not allowed.
 * See License.txt file that comes with this distribution
 *
 * Description: 
 * Equality solver. Implements Gaussian elimination.
 * Return system of equalities without the eliminated variable.
 * The size of the output matrix is equal to the size of the resulting matrix
 *
 * Author: Vitaly German <vitaly.german@gmail.com>, (C) 2006-2007
 * 
 **/

#pragma once
//#define DEBUG_EQS

#ifdef DEBUG_EQS
#include <iostream>

using std::cout;
using std::endl;
using std::flush;
#endif

template <typename MX, typename C>
class  EqSolve;

/// =================== smatrix2::sparse_matrix2 specialization ==================

template <typename C>
class  EqSolve<typename smatrix2::sparse_matrix2<typename C::value_type>, C>{
    typedef typename smatrix2::sparse_matrix2<typename C::value_type> MX;
    MX *mx;
    C * cnt;
public:
    
    EqSolve(MX& m, C & c):mx(&m),cnt(&c){}
    EqSolve():mx(0),cnt(0){}
    void Accept(MX& mx, C & c);
    void Solve(MX& m, C & c, size_t line_id, size_t var_id);
    void Solve(size_t line_id, size_t var_id) const;
    
    typedef typename C::value_type value_type;
private:
    
    typedef std::pair<size_t, value_type> Pos2Coef;
    typedef std::vector<Pos2Coef> VecPos2Coef;
    
    VecPos2Coef& build_signs(VecPos2Coef&,const size_t& line_id, const size_t& var_id) const;
    
};

template < typename C>
inline void EqSolve<smatrix2::sparse_matrix2<typename C::value_type>, C>::Accept(MX& m, C & c){
    mx = &m;
    cnt =  &c;
}

template < typename C>
inline void EqSolve<smatrix2::sparse_matrix2<typename C::value_type>, C>::Solve(MX &m, C& c, size_t line_id, size_t var_id){
    this->Accept(m, c);
    return Solve(line_id, var_id);
};

/// For other then smatrix matrices
/// TODO: finish

template <typename C>
void EqSolve<smatrix2::sparse_matrix2<typename C::value_type>, C>::Solve(size_t line_id, size_t var_id) const{

#ifdef DEBUG_EQS
    cout<<"\nEliminateEquVariable: eliminate var at pos (" <<line_id<<", "<<var_id<<")"<<flush;
#endif

    // divide every variable for the coeficient of the variable
    // to be eliminated

    for(typename MX::iterator1 it1=mx->begin1(var_id); it1; ++it1)
    {

        typename MX::value_type local_k = (*it1<0)? -1*(*it1):*it1; // got non zero coefficient, take modulo
#ifdef DEBUG_EQS
        cout<<"\nlocal_k = "<<local_k; 
#endif

        for(typename MX::iterator2 it2=mx->begin2(it1.get_s1()); it2; ++it2)
        { 
        
    	    *it2 = *it2/local_k;
        }
        (*cnt)[it1.get_s1()] = (*cnt)[it1.get_s1()]/local_k;
    }


/**
     1. take given coefficient with the opposite signs
     2. add the coefficient to other coefficient for given varieble
        Note. the givel equality line automaticaly deletes itself (becomes 0)
 */ 
 
   VecPos2Coef master_sign;
    master_sign =  build_signs(master_sign, line_id, var_id);

    /// for all columns
    typename MX::value_type update_val;
    size_t update_line;
    /// updating coefficients
    for(typename MX::iterator2 it2=mx->begin2(line_id); it2; ++it2)
    {
        typename MX::value_type local_k = *it2; 

#ifdef DEBUG_EQS
        cout<<"\nlocal_k("<<it2.get_s1()<<", "<<it2.get_s2()<<") = "<<local_k;
#endif
        for(size_t k = 0 ; k < master_sign.size(); k++){
            update_val = local_k * master_sign[k].second;
            update_line = master_sign[k].first;
            (*mx)(update_line, it2.get_s2()) += update_val;
        }
    }
   
   /// updating constants
   typename MX::value_type local_k = (*cnt)[line_id];
   for(size_t k = 0 ; k < master_sign.size(); k++){
       update_val = local_k * master_sign[k].second;
       update_line = master_sign[k].first;
       (*cnt)[update_line] += update_val;
   }
    return;
}

/**
 *
 * Builds vector[line_id] == -1 if line_id ineaquality coeficient of the variable to be
 * eliminated has the same sign as coeficient of the variable in a eliminating equality
 * If the coefficients are differnt vector[line_id] == 1.
 *
 */


template <typename C>
typename EqSolve<smatrix2::sparse_matrix2<typename C::value_type>, C>::VecPos2Coef & 
EqSolve<smatrix2::sparse_matrix2<typename C::value_type>,C>::build_signs(
	typename EqSolve<smatrix2::sparse_matrix2<typename C::value_type>,C>::VecPos2Coef & m_vector, 
	const size_t & line_id, const size_t  &var_id) const
{

    typename C::value_type mult = -1;
    if ((*mx)(line_id, var_id) < 0)
    	mult = 1;
    for(typename MX::const_iterator1 it1=mx->begin1(var_id); it1; ++it1)
    {
    	if (*it1 < 0) 
	{
    	    m_vector.push_back(typename VecPos2Coef::value_type(it1.get_s1(), -1*mult));
    	} else { 
    	    m_vector.push_back(typename VecPos2Coef::value_type(it1.get_s1(), 1*mult));
    	}
    }
    return m_vector;
};

/// =================== Generic matrix =====================================

template <typename MX, typename C>
class  EqSolve{
    MX *mx;
    C * cnt;
public:
    EqSolve(MX& m, C & c):mx(&m),cnt(&c){}
    EqSolve():mx(0),cnt(0){}
    void Accept(MX& mx, C & c);
    void Solve(MX& m, C & c, size_t line_id, size_t var_id);
    void Solve(size_t line_id, size_t var_id) const;
    
    typedef typename C::value_type value_type;
private:
    
    typedef std::pair<size_t, value_type> Pos2Coef;
    typedef std::vector<Pos2Coef> VecPos2Coef;
    
    VecPos2Coef& build_signs(VecPos2Coef&,const size_t& line_id, const size_t& var_id) const;
    
};


template <typename MX, typename C>
inline void EqSolve< MX, C>::Accept(MX& m, C & c){
    mx = &m;
    cnt =  &c;
}

template <typename MX, typename C>
inline void EqSolve< MX, C>::Solve(MX &m, C& c, size_t line_id, size_t var_id){
    this->Accept(m, c);
    return Solve(line_id, var_id);
};



template <typename MX, typename C>
void EqSolve< MX, C>::Solve(size_t line_id, size_t var_id) const{

#ifdef DEBUG_EQS
    cout<<"\nEqSolve.Solve at pos (" <<line_id<<", "<<var_id<<")"<<flush;
#endif

    // devide every variable for the coeficient of the variable
    // to be eliminated
    const typename C::value_type zero = typename C::value_type (0);

    for(typename MX::iterator1 it1=(mx->begin2()+var_id).begin(); it1!=(mx->begin2()+var_id).end(); ++it1)
    {
        typename MX::value_type local_k = (*it1< zero)? -1*(*it1):*it1;
        if (local_k == zero ) 
        	continue;
#ifdef DEBUG_EQS
        cout<<"\nlocal_k = "<<local_k; 
#endif
	/// div matrix
        for(typename MX::iterator2 it2=it1.begin(); it2!=it1.end(); ++it2)
        { 
    	    *it2 = *it2/local_k;
        }
	/// div vector
        (*cnt)[it1.index1()] = (*cnt)[it1.index1()]/local_k;

    }


///     1. take given coefficient with the opposite signs
///     2. add the coefficient to other coefficient for given varieble
///        Note. the givel equality line automaticaly deletes itself (becomes 0)

 
   VecPos2Coef master_sign;
    master_sign =  build_signs(master_sign, line_id, var_id);

    /// for all columns
    typename MX::value_type update_val;
    size_t update_line;
    /// updating coefficients
    for(typename MX::iterator2 it2=(mx->begin1()+line_id).begin(); it2!=(mx->begin1()+line_id).end() ; ++it2)
    {
        typename MX::value_type local_k = *it2; 
        if (local_k == zero)
        	continue;
#ifdef DEBUG_EQS
        cout<<"\nlocal_k("<<it2.index1()<<", "<<it2.index2()<<") = "<<local_k;
#endif
        for(size_t k = 0 ; k < master_sign.size(); k++){
            update_val = local_k * master_sign[k].second;
            update_line = master_sign[k].first;

            (*mx)(update_line, it2.index2()) += update_val;

        }
    }
   
   /// updating constants
   typename MX::value_type local_k = (*cnt)[line_id];
   for(size_t k = 0 ; k < master_sign.size(); k++){
       update_val = local_k * master_sign[k].second;
       update_line = master_sign[k].first;
       (*cnt)[update_line] += update_val;
   }
    return;
}



template <typename MX, typename C>
typename EqSolve<MX, C>::VecPos2Coef & 
EqSolve<MX, C>::build_signs(
	typename EqSolve<MX ,C>::VecPos2Coef & m_vector, 
	const size_t & line_id, const size_t  &var_id) const
{

    typename C::value_type mult = -1;
    const typename C::value_type zero = typename C::value_type(0);
    if ((*mx)(line_id, var_id) < zero )
    	mult = 1;

    for(typename MX::const_iterator1 it1=(mx->begin2()+ var_id).begin(); it1!=(mx->begin2()+ var_id).end(); ++it1)
    {
    	if (*it1 == zero)
    	    continue;
    	if (*it1 < zero) 
	{
    	    m_vector.push_back(typename VecPos2Coef::value_type(it1.index1(), -1*mult));
    	} else { 
    	    m_vector.push_back(typename VecPos2Coef::value_type(it1.index1(), 1*mult));
    	}

    }
    return m_vector;
};

